/*
 * inv_porting.c
 *
 *  Created on: Apr 8, 2021
 *      Author: hemingway
 */
//#include "inv_mpu.h"
//#include "inv_mpu_dmp_motion_driver.h"
//#include "invensense.h"
//#include "invensense_adv.h"
//#include "eMPL_outputs.h"
//#include "mltypes.h"
//#include "mpu.h"
//
///* Starting sampling rate. */
//#define DEFAULT_MPU_HZ  (20)

//typedef enum {
//    PACKET_DATA_ACCEL = 0,
//    PACKET_DATA_GYRO,
//    PACKET_DATA_COMPASS,
//    PACKET_DATA_QUAT,
//    PACKET_DATA_EULER,
//    PACKET_DATA_ROT,
//    PACKET_DATA_HEADING,
//    PACKET_DATA_LINEAR_ACCEL,
//    NUM_DATA_PACKETS
//} eMPL_packet_e;
//
///* Data read from MPL. */
//#define PRINT_ACCEL     (0x01)
//#define PRINT_GYRO      (0x02)
//#define PRINT_QUAT      (0x04)
//#define PRINT_COMPASS   (0x08)
//#define PRINT_EULER     (0x10)
//#define PRINT_ROT_MAT   (0x20)
//#define PRINT_HEADING   (0x40)
//#define PRINT_PEDO      (0x80)
//#define PRINT_LINEAR_ACCEL (0x100)
//#define PRINT_GRAVITY_VECTOR (0x200)
//
//#define ACCEL_ON        (0x01)
//#define GYRO_ON         (0x02)
//#define COMPASS_ON      (0x04)
//
//#define MOTION          (0)
//#define NO_MOTION       (1)
//
//#define PEDO_READ_MS    (1000)
//#define TEMP_READ_MS    (500)
//#define COMPASS_READ_MS (100)
//

//
//struct rx_s {
//    unsigned char header[3];
//    unsigned char cmd;
//};
//
//struct hal_s {
//    unsigned char lp_accel_mode;
//    unsigned char sensors;
//    unsigned char dmp_on;
//    unsigned char wait_for_tap;
//    volatile unsigned char new_gyro;
//    unsigned char motion_int_mode;
//    unsigned long no_dmp_hz;
//    unsigned long next_pedo_ms;
//    unsigned long next_temp_ms;
//    unsigned long next_compass_ms;
//    unsigned int report;
//    unsigned short dmp_features;
//    struct rx_s rx;
//};
//static struct hal_s hal = {0};
//
//unsigned char *mpl_key = (unsigned char*)"eMPL 5.1";
//
///* Platform-specific information. Kinda like a boardfile. */
//struct platform_data_s {
//    signed char orientation[9];
//};
//
///* The sensors can be mounted onto the board in any orientation. The mounting
// * matrix seen below tells the MPL how to rotate the raw data from the
// * driver(s).
// * TODO: The following matrices refer to the configuration on internal test
// * boards at Invensense. If needed, please modify the matrices to match the
// * chip-to-body matrix for your particular set up.
// */
//static struct platform_data_s gyro_pdata = {
//    .orientation = { 1, 0, 0,
//                     0, 1, 0,
//                     0, 0, 1}
//};
//
//#define BUF_SIZE        (256)
//#define PACKET_LENGTH   (23)
//
//#define PACKET_DEBUG    (1)
//#define PACKET_QUAT     (2)
//#define PACKET_DATA     (3)
//
//#define MLP_PUTS(x,y)                         H_CC2640R2F_UART_TxBuff(x,y)
//
//int _MLPrintLog (int priority, const char* tag, const char* fmt, ...)
//{
//    va_list args;
//    int length, ii, i;
//    char buf[BUF_SIZE], out[PACKET_LENGTH], this_length;
//
//    /* This can be modified to exit for unsupported priorities. */
//    switch (priority) {
//    case MPL_LOG_UNKNOWN:
//    case MPL_LOG_DEFAULT:
//    case MPL_LOG_VERBOSE:
//    case MPL_LOG_DEBUG:
//    case MPL_LOG_INFO:
//    case MPL_LOG_WARN:
//    case MPL_LOG_ERROR:
//    case MPL_LOG_SILENT:
//        break;
//    default:
//        return 0;
//    }
//
//    va_start(args, fmt);
//
//    length = vsprintf(buf, fmt, args);
//    if (length <= 0) {
//        va_end(args);
//        return length;
//    }
//
//    memset(out, 0, PACKET_LENGTH);
//    out[0] = '$';
//    out[1] = PACKET_DEBUG;
//    out[2] = priority;
//    out[21] = '\r';
//    out[22] = '\n';
//    for (ii = 0; ii < length; ii += (PACKET_LENGTH-5)) {
//#define min(a,b) ((a < b) ? a : b)
//        this_length = min(length-ii, PACKET_LENGTH-5);
//        memset(out+3, 0, 18);
//        memcpy(out+3, buf+ii, this_length);
////        for (i=0; i<PACKET_LENGTH; i++) {
////          fputc(out[i]);
////        }
//        MLP_PUTS(out, PACKET_LENGTH);
//    }
//
//
//    va_end(args);
//
//    return 0;
//}
//
//void eMPL_send_quat(long *quat)
//{
//    char out[PACKET_LENGTH];
//    int i;
//    if (!quat)
//        return;
//    memset(out, 0, PACKET_LENGTH);
//    out[0] = '$';
//    out[1] = PACKET_QUAT;
//    out[3] = (char)(quat[0] >> 24);
//    out[4] = (char)(quat[0] >> 16);
//    out[5] = (char)(quat[0] >> 8);
//    out[6] = (char)quat[0];
//    out[7] = (char)(quat[1] >> 24);
//    out[8] = (char)(quat[1] >> 16);
//    out[9] = (char)(quat[1] >> 8);
//    out[10] = (char)quat[1];
//    out[11] = (char)(quat[2] >> 24);
//    out[12] = (char)(quat[2] >> 16);
//    out[13] = (char)(quat[2] >> 8);
//    out[14] = (char)quat[2];
//    out[15] = (char)(quat[3] >> 24);
//    out[16] = (char)(quat[3] >> 16);
//    out[17] = (char)(quat[3] >> 8);
//    out[18] = (char)quat[3];
//    out[21] = '\r';
//    out[22] = '\n';
//
////    for (i=0; i<PACKET_LENGTH; i++) {
////      fputc(out[i]);
////    }
//    MLP_PUTS(out, PACKET_LENGTH);
//}
//
//void eMPL_send_data(unsigned char type, long *data)
//{
//    char out[PACKET_LENGTH];
//    int i;
//    if (!data)
//        return;
//    memset(out, 0, PACKET_LENGTH);
//    out[0] = '$';
//    out[1] = PACKET_DATA;
//    out[2] = type;
//    out[21] = '\r';
//    out[22] = '\n';
//    switch (type) {
//    /* Two bytes per-element. */
//    case PACKET_DATA_ROT:
//        out[3] = (char)(data[0] >> 24);
//        out[4] = (char)(data[0] >> 16);
//        out[5] = (char)(data[1] >> 24);
//        out[6] = (char)(data[1] >> 16);
//        out[7] = (char)(data[2] >> 24);
//        out[8] = (char)(data[2] >> 16);
//        out[9] = (char)(data[3] >> 24);
//        out[10] = (char)(data[3] >> 16);
//        out[11] = (char)(data[4] >> 24);
//        out[12] = (char)(data[4] >> 16);
//        out[13] = (char)(data[5] >> 24);
//        out[14] = (char)(data[5] >> 16);
//        out[15] = (char)(data[6] >> 24);
//        out[16] = (char)(data[6] >> 16);
//        out[17] = (char)(data[7] >> 24);
//        out[18] = (char)(data[7] >> 16);
//        out[19] = (char)(data[8] >> 24);
//        out[20] = (char)(data[8] >> 16);
//        break;
//    /* Four bytes per-element. */
//    /* Four elements. */
//    case PACKET_DATA_QUAT:
//        out[15] = (char)(data[3] >> 24);
//        out[16] = (char)(data[3] >> 16);
//        out[17] = (char)(data[3] >> 8);
//        out[18] = (char)data[3];
//    /* Three elements. */
//    case PACKET_DATA_ACCEL:
//    case PACKET_DATA_GYRO:
//    case PACKET_DATA_COMPASS:
//    case PACKET_DATA_EULER:
//        out[3] = (char)(data[0] >> 24);
//        out[4] = (char)(data[0] >> 16);
//        out[5] = (char)(data[0] >> 8);
//        out[6] = (char)data[0];
//        out[7] = (char)(data[1] >> 24);
//        out[8] = (char)(data[1] >> 16);
//        out[9] = (char)(data[1] >> 8);
//        out[10] = (char)data[1];
//        out[11] = (char)(data[2] >> 24);
//        out[12] = (char)(data[2] >> 16);
//        out[13] = (char)(data[2] >> 8);
//        out[14] = (char)data[2];
//        break;
//    case PACKET_DATA_HEADING:
//        out[3] = (char)(data[0] >> 24);
//        out[4] = (char)(data[0] >> 16);
//        out[5] = (char)(data[0] >> 8);
//        out[6] = (char)data[0];
//        break;
//    default:
//        return;
//    }
////    for (i=0; i<PACKET_LENGTH; i++) {
////      fputc(out[i]);
////    }
//    MLP_PUTS(out, PACKET_LENGTH);
//}
//
///* Get data from MPL.
// * TODO: Add return values to the inv_get_sensor_type_xxx APIs to differentiate
// * between new and stale data.
// */
//static void read_from_mpl(void)
//{
//    long msg, data[9];
//    int8_t accuracy;
//    unsigned long timestamp;
//    float float_data[3] = {0};
//
//    if (inv_get_sensor_type_quat(data, &accuracy, (inv_time_t*)&timestamp)) {
//       /* Sends a quaternion packet to the PC. Since this is used by the Python
//        * test app to visually represent a 3D quaternion, it's sent each time
//        * the MPL has new data.
//        */
//        eMPL_send_quat(data);
//
//        /* Specific data packets can be sent or suppressed using USB commands. */
//        if (hal.report & PRINT_QUAT)
//            eMPL_send_data(PACKET_DATA_QUAT, data);
//    }
//
//    if (hal.report & PRINT_ACCEL) {
//        if (inv_get_sensor_type_accel(data, &accuracy,
//            (inv_time_t*)&timestamp))
//            eMPL_send_data(PACKET_DATA_ACCEL, data);
//    }
//    if (hal.report & PRINT_GYRO) {
//        if (inv_get_sensor_type_gyro(data, &accuracy,
//            (inv_time_t*)&timestamp))
//            eMPL_send_data(PACKET_DATA_GYRO, data);
//    }
//#ifdef COMPASS_ENABLED
//    if (hal.report & PRINT_COMPASS) {
//        if (inv_get_sensor_type_compass(data, &accuracy,
//            (inv_time_t*)&timestamp))
//            eMPL_send_data(PACKET_DATA_COMPASS, data);
//    }
//#endif
//    if (hal.report & PRINT_EULER) {
//        if (inv_get_sensor_type_euler(data, &accuracy,
//            (inv_time_t*)&timestamp))
//            eMPL_send_data(PACKET_DATA_EULER, data);
//    }
//    if (hal.report & PRINT_ROT_MAT) {
//        if (inv_get_sensor_type_rot_mat(data, &accuracy,
//            (inv_time_t*)&timestamp))
//            eMPL_send_data(PACKET_DATA_ROT, data);
//    }
//    if (hal.report & PRINT_HEADING) {
//        if (inv_get_sensor_type_heading(data, &accuracy,
//            (inv_time_t*)&timestamp))
//            eMPL_send_data(PACKET_DATA_HEADING, data);
//    }
//    if (hal.report & PRINT_LINEAR_ACCEL) {
//        if (inv_get_sensor_type_linear_acceleration(float_data, &accuracy, (inv_time_t*)&timestamp)) {
//            printf("Linear Accel: %7.5f %7.5f %7.5f\r\n",
//                    float_data[0], float_data[1], float_data[2]);
//         }
//    }
//    if (hal.report & PRINT_GRAVITY_VECTOR) {
//            if (inv_get_sensor_type_gravity(float_data, &accuracy,
//                (inv_time_t*)&timestamp))
//                printf("Gravity Vector: %7.5f %7.5f %7.5f\r\n",
//                        float_data[0], float_data[1], float_data[2]);
//    }
//    if (hal.report & PRINT_PEDO) {
//        unsigned long timestamp;
//        get_tick_count(&timestamp);
//        if (timestamp > hal.next_pedo_ms) {
//            hal.next_pedo_ms = timestamp + PEDO_READ_MS;
//            unsigned long step_count, walk_time;
//            dmp_get_pedometer_step_count(&step_count);
//            dmp_get_pedometer_walk_time(&walk_time);
//            printf("Walked %ld steps over %ld milliseconds..\n", step_count,
//            walk_time);
//        }
//    }
//
//    /* Whenever the MPL detects a change in motion state, the application can
//     * be notified. For this example, we use an LED to represent the current
//     * motion state.
//     */
//    msg = inv_get_message_level_0(INV_MSG_MOTION_EVENT |
//            INV_MSG_NO_MOTION_EVENT);
//    if (msg) {
//        if (msg & INV_MSG_MOTION_EVENT) {
//            printf("Motion!\n");
//        } else if (msg & INV_MSG_NO_MOTION_EVENT) {
//            printf("No motion!\n");
//        }
//    }
//}
//
//static void tap_cb(unsigned char direction, unsigned char count)
//{
//    switch (direction) {
//    case TAP_X_UP:
//        printf("Tap X+ ");
//        break;
//    case TAP_X_DOWN:
//        printf("Tap X- ");
//        break;
//    case TAP_Y_UP:
//        printf("Tap Y+ ");
//        break;
//    case TAP_Y_DOWN:
//        printf("Tap Y- ");
//        break;
//    case TAP_Z_UP:
//        printf("Tap Z+ ");
//        break;
//    case TAP_Z_DOWN:
//        printf("Tap Z- ");
//        break;
//    default:
//        return;
//    }
//    printf("x%d\n", count);
//    return;
//}
//
//static void android_orient_cb(unsigned char orientation)
//{
//    switch (orientation) {
//    case ANDROID_ORIENT_PORTRAIT:
//        printf("Portrait\n");
//        break;
//    case ANDROID_ORIENT_LANDSCAPE:
//        printf("Landscape\n");
//        break;
//    case ANDROID_ORIENT_REVERSE_PORTRAIT:
//        printf("Reverse Portrait\n");
//        break;
//    case ANDROID_ORIENT_REVERSE_LANDSCAPE:
//        printf("Reverse Landscape\n");
//        break;
//    default:
//        return;
//    }
//}
//
//void inv_dmp_init(void)
//{
//    inv_error_t result;
//    unsigned char accel_fsr,  new_temp = 0;
//    unsigned short gyro_rate, gyro_fsr;
//    unsigned long timestamp;
////    struct int_param_s int_param;
//
//#ifdef COMPASS_ENABLED
//    unsigned char new_compass = 0;
//    unsigned short compass_fsr;
//#endif
//
//  result = mpu_init(NULL);
//  if (result) {
//      printf("Could not initialize gyro.\n");
//  }
//
//
//    /* If you're not using an MPU9150 AND you're not using DMP features, this
//     * function will place all slaves on the primary bus.
//     * mpu_set_bypass(1);
//     */
//
//  result = inv_init_mpl();
//  if (result) {
//      printf("Could not initialize MPL.\n");
//  }
//
//    /* Compute 6-axis and 9-axis quaternions. */
//    inv_enable_quaternion();
////    inv_enable_9x_sensor_fusion();
//    /* The MPL expects compass data at a constant rate (matching the rate
//     * passed to inv_set_compass_sample_rate). If this is an issue for your
//     * application, call this function, and the MPL will depend on the
//     * timestamps passed to inv_build_compass instead.
//     *
//     * inv_9x_fusion_use_timestamps(1);
//     */
//
////    /* This function has been deprecated.
////     * inv_enable_no_gyro_fusion();
////     */
////
////    /* Update gyro biases when not in motion.
////     * WARNING: These algorithms are mutually exclusive.
////     */
////    inv_enable_fast_nomot();
////    /* inv_enable_motion_no_motion(); */
////    /* inv_set_no_motion_time(1000); */
////
////    /* Update gyro biases when temperature changes. */
////    inv_enable_gyro_tc();
////
////    /* This algorithm updates the accel biases when in motion. A more accurate
////     * bias measurement can be made when running the self-test (see case 't' in
////     * handle_input), but this algorithm can be enabled if the self-test can't
////     * be executed in your application.
////     *
////     * inv_enable_in_use_auto_calibration();
////     */
////#ifdef COMPASS_ENABLED
////    /* Compass calibration algorithms. */
////    inv_enable_vector_compass_cal();
////    inv_enable_magnetic_disturbance();
////#endif
////    /* If you need to estimate your heading before the compass is calibrated,
////     * enable this algorithm. It becomes useless after a good figure-eight is
////     * detected, so we'll just leave it out to save memory.
////     * inv_enable_heading_from_gyro();
////     */
////
////    /* Allows use of the MPL APIs in read_from_mpl. */
////    inv_enable_eMPL_outputs();
////
////  result = inv_start_mpl();
////  if (result == INV_ERROR_NOT_AUTHORIZED) {
////      while (1) {
////          printf("Not authorized.\n");
////      }
////  }
////  if (result) {
////      printf("Could not start the MPL.\n");
////  }
//
////    /* Get/set hardware configuration. Start gyro. */
////    /* Wake up all sensors. */
////#ifdef COMPASS_ENABLED
////    mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
////#else
////    mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
////#endif
////    /* Push both gyro and accel data into the FIFO. */
////    mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
////    mpu_set_sample_rate(DEFAULT_MPU_HZ);
////#ifdef COMPASS_ENABLED
////    /* The compass sampling rate can be less than the gyro/accel sampling rate.
////     * Use this function for proper power management.
////     */
////    mpu_set_compass_sample_rate(1000 / COMPASS_READ_MS);
////#endif
////    /* Read back configuration in case it was set improperly. */
////    mpu_get_sample_rate(&gyro_rate);
////    mpu_get_gyro_fsr(&gyro_fsr);
////    mpu_get_accel_fsr(&accel_fsr);
////#ifdef COMPASS_ENABLED
////    mpu_get_compass_fsr(&compass_fsr);
////#endif
////    /* Sync driver configuration with MPL. */
////    /* Sample rate expected in microseconds. */
////    inv_set_gyro_sample_rate(1000000L / gyro_rate);
////    inv_set_accel_sample_rate(1000000L / gyro_rate);
////#ifdef COMPASS_ENABLED
////    /* The compass rate is independent of the gyro and accel rates. As long as
////     * inv_set_compass_sample_rate is called with the correct value, the 9-axis
////     * fusion algorithm's compass correction gain will work properly.
////     */
////    inv_set_compass_sample_rate(COMPASS_READ_MS * 1000L);
////#endif
////    /* Set chip-to-body orientation matrix.
////     * Set hardware units to dps/g's/degrees scaling factor.
////     */
////    inv_set_gyro_orientation_and_scale(
////            inv_orientation_matrix_to_scalar(gyro_pdata.orientation),
////            (long)gyro_fsr<<15);
////    inv_set_accel_orientation_and_scale(
////            inv_orientation_matrix_to_scalar(gyro_pdata.orientation),
////            (long)accel_fsr<<15);
////#ifdef COMPASS_ENABLED
////    inv_set_compass_orientation_and_scale(
////            inv_orientation_matrix_to_scalar(compass_pdata.orientation),
////            (long)compass_fsr<<15);
////#endif
////    /* Initialize HAL state variables. */
////#ifdef COMPASS_ENABLED
////    hal.sensors = ACCEL_ON | GYRO_ON | COMPASS_ON;
////#else
////    hal.sensors = ACCEL_ON | GYRO_ON;
////#endif
////    hal.dmp_on = 0;
////    hal.report = 0;
////    hal.rx.cmd = 0;
////    hal.next_pedo_ms = 0;
////    hal.next_compass_ms = 0;
////    hal.next_temp_ms = 0;
////
////  /* Compass reads are handled by scheduler. */
////  get_tick_count(&timestamp);
//
////    /* To initialize the DMP:
////     * 1. Call dmp_load_motion_driver_firmware(). This pushes the DMP image in
////     *    inv_mpu_dmp_motion_driver.h into the MPU memory.
////     * 2. Push the gyro and accel orientation matrix to the DMP.
////     * 3. Register gesture callbacks. Don't worry, these callbacks won't be
////     *    executed unless the corresponding feature is enabled.
////     * 4. Call dmp_enable_feature(mask) to enable different features.
////     * 5. Call dmp_set_fifo_rate(freq) to select a DMP output rate.
////     * 6. Call any feature-specific control functions.
////     *
////     * To enable the DMP, just call mpu_set_dmp_state(1). This function can
////     * be called repeatedly to enable and disable the DMP at runtime.
////     *
////     * The following is a short summary of the features supported in the DMP
////     * image provided in inv_mpu_dmp_motion_driver.c:
////     * DMP_FEATURE_LP_QUAT: Generate a gyro-only quaternion on the DMP at
////     * 200Hz. Integrating the gyro data at higher rates reduces numerical
////     * errors (compared to integration on the MCU at a lower sampling rate).
////     * DMP_FEATURE_6X_LP_QUAT: Generate a gyro/accel quaternion on the DMP at
////     * 200Hz. Cannot be used in combination with DMP_FEATURE_LP_QUAT.
////     * DMP_FEATURE_TAP: Detect taps along the X, Y, and Z axes.
////     * DMP_FEATURE_ANDROID_ORIENT: Google's screen rotation algorithm. Triggers
////     * an event at the four orientations where the screen should rotate.
////     * DMP_FEATURE_GYRO_CAL: Calibrates the gyro data after eight seconds of
////     * no motion.
////     * DMP_FEATURE_SEND_RAW_ACCEL: Add raw accelerometer data to the FIFO.
////     * DMP_FEATURE_SEND_RAW_GYRO: Add raw gyro data to the FIFO.
////     * DMP_FEATURE_SEND_CAL_GYRO: Add calibrated gyro data to the FIFO. Cannot
////     * be used in combination with DMP_FEATURE_SEND_RAW_GYRO.
////     */
////    dmp_load_motion_driver_firmware();
////    dmp_set_orientation(
////        inv_orientation_matrix_to_scalar(gyro_pdata.orientation));
////    dmp_register_tap_cb(tap_cb);
////    dmp_register_android_orient_cb(android_orient_cb);
////    /*
////     * Known Bug -
////     * DMP when enabled will sample sensor data at 200Hz and output to FIFO at the rate
////     * specified in the dmp_set_fifo_rate API. The DMP will then sent an interrupt once
////     * a sample has been put into the FIFO. Therefore if the dmp_set_fifo_rate is at 25Hz
////     * there will be a 25Hz interrupt from the MPU device.
////     *
////     * There is a known issue in which if you do not enable DMP_FEATURE_TAP
////     * then the interrupts will be at 200Hz even if fifo rate
////     * is set at a different rate. To avoid this issue include the DMP_FEATURE_TAP
////     *
////     * DMP sensor fusion works only with gyro at +-2000dps and accel +-2G
////     */
////    hal.dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
////        DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
////        DMP_FEATURE_GYRO_CAL;
////    dmp_enable_feature(hal.dmp_features);
////    dmp_set_fifo_rate(DEFAULT_MPU_HZ);
////    mpu_set_dmp_state(1);
////    hal.dmp_on = 1;
//
////  while(1){
////
////    unsigned long sensor_timestamp;
////    int new_data = 0;
//////    if (USART_GetITStatus(USART2, USART_IT_RXNE)) {
//////        /* A byte has been received via USART. See handle_input for a list of
//////         * valid commands.
//////         */
//////        USART_ClearITPendingBit(USART2, USART_IT_RXNE);
//////        handle_input();
//////    }
////    get_tick_count(&timestamp);
////
////#ifdef COMPASS_ENABLED
////        /* We're not using a data ready interrupt for the compass, so we'll
////         * make our compass reads timer-based instead.
////         */
////        if ((timestamp > hal.next_compass_ms) && !hal.lp_accel_mode &&
////            hal.new_gyro && (hal.sensors & COMPASS_ON)) {
////            hal.next_compass_ms = timestamp + COMPASS_READ_MS;
////            new_compass = 1;
////        }
////#endif
////        /* Temperature data doesn't need to be read with every gyro sample.
////         * Let's make them timer-based like the compass reads.
////         */
////        if (timestamp > hal.next_temp_ms) {
////            hal.next_temp_ms = timestamp + TEMP_READ_MS;
////            new_temp = 1;
////        }
////
////    if (hal.motion_int_mode) {
////        /* Enable motion interrupt. */
////        mpu_lp_motion_interrupt(500, 1, 5);
////        /* Notify the MPL that contiguity was broken. */
////        inv_accel_was_turned_off();
////        inv_gyro_was_turned_off();
////        inv_compass_was_turned_off();
////        inv_quaternion_sensor_was_turned_off();
////        /* Wait for the MPU interrupt. */
////        while (!hal.new_gyro) {}
////        /* Restore the previous sensor configuration. */
////        mpu_lp_motion_interrupt(0, 0, 0);
////        hal.motion_int_mode = 0;
////    }
////
////    if (!hal.sensors || !hal.new_gyro) {
////        continue;
////    }
////
////        if (hal.new_gyro && hal.lp_accel_mode) {
////            short accel_short[3];
////            long accel[3];
////            mpu_get_accel_reg(accel_short, &sensor_timestamp);
////            accel[0] = (long)accel_short[0];
////            accel[1] = (long)accel_short[1];
////            accel[2] = (long)accel_short[2];
////            inv_build_accel(accel, 0, sensor_timestamp);
////            new_data = 1;
////            hal.new_gyro = 0;
////        } else if (hal.new_gyro && hal.dmp_on) {
////            short gyro[3], accel_short[3], sensors;
////            unsigned char more;
////            long accel[3], quat[4], temperature;
////            /* This function gets new data from the FIFO when the DMP is in
////             * use. The FIFO can contain any combination of gyro, accel,
////             * quaternion, and gesture data. The sensors parameter tells the
////             * caller which data fields were actually populated with new data.
////             * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
////             * the FIFO isn't being filled with accel data.
////             * The driver parses the gesture data to determine if a gesture
////             * event has occurred; on an event, the application will be notified
////             * via a callback (assuming that a callback function was properly
////             * registered). The more parameter is non-zero if there are
////             * leftover packets in the FIFO.
////             */
////            dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors, &more);
////            if (!more)
////                hal.new_gyro = 0;
////            if (sensors & INV_XYZ_GYRO) {
////                /* Push the new data to the MPL. */
////                inv_build_gyro(gyro, sensor_timestamp);
////                new_data = 1;
////                if (new_temp) {
////                    new_temp = 0;
////                    /* Temperature only used for gyro temp comp. */
////                    mpu_get_temperature(&temperature, &sensor_timestamp);
////                    inv_build_temp(temperature, sensor_timestamp);
////                }
////            }
////            if (sensors & INV_XYZ_ACCEL) {
////                accel[0] = (long)accel_short[0];
////                accel[1] = (long)accel_short[1];
////                accel[2] = (long)accel_short[2];
////                inv_build_accel(accel, 0, sensor_timestamp);
////                new_data = 1;
////            }
////            if (sensors & INV_WXYZ_QUAT) {
////                inv_build_quat(quat, 0, sensor_timestamp);
////                new_data = 1;
////            }
////        } else if (hal.new_gyro) {
////            short gyro[3], accel_short[3];
////            unsigned char sensors, more;
////            long accel[3], temperature;
////            /* This function gets new data from the FIFO. The FIFO can contain
////             * gyro, accel, both, or neither. The sensors parameter tells the
////             * caller which data fields were actually populated with new data.
////             * For example, if sensors == INV_XYZ_GYRO, then the FIFO isn't
////             * being filled with accel data. The more parameter is non-zero if
////             * there are leftover packets in the FIFO. The HAL can use this
////             * information to increase the frequency at which this function is
////             * called.
////             */
////            hal.new_gyro = 0;
////            mpu_read_fifo(gyro, accel_short, &sensor_timestamp,
////                &sensors, &more);
////            if (more)
////                hal.new_gyro = 1;
////            if (sensors & INV_XYZ_GYRO) {
////                /* Push the new data to the MPL. */
////                inv_build_gyro(gyro, sensor_timestamp);
////                new_data = 1;
////                if (new_temp) {
////                    new_temp = 0;
////                    /* Temperature only used for gyro temp comp. */
////                    mpu_get_temperature(&temperature, &sensor_timestamp);
////                    inv_build_temp(temperature, sensor_timestamp);
////                }
////            }
////            if (sensors & INV_XYZ_ACCEL) {
////                accel[0] = (long)accel_short[0];
////                accel[1] = (long)accel_short[1];
////                accel[2] = (long)accel_short[2];
////                inv_build_accel(accel, 0, sensor_timestamp);
////                new_data = 1;
////            }
////        }
////#ifdef COMPASS_ENABLED
////        if (new_compass) {
////            short compass_short[3];
////            long compass[3];
////            new_compass = 0;
////            /* For any MPU device with an AKM on the auxiliary I2C bus, the raw
////             * magnetometer registers are copied to special gyro registers.
////             */
////            if (!mpu_get_compass_reg(compass_short, &sensor_timestamp)) {
////                compass[0] = (long)compass_short[0];
////                compass[1] = (long)compass_short[1];
////                compass[2] = (long)compass_short[2];
////                /* NOTE: If using a third-party compass calibration library,
////                 * pass in the compass data in uT * 2^16 and set the second
////                 * parameter to INV_CALIBRATED | acc, where acc is the
////                 * accuracy from 0 to 3.
////                 */
////                inv_build_compass(compass, 0, sensor_timestamp);
////            }
////            new_data = 1;
////        }
////#endif
////        if (new_data) {
////            inv_execute_on_data();
////            /* This function reads bias-compensated sensor data and sensor
////             * fusion outputs from the MPL. The outputs are formatted as seen
////             * in eMPL_outputs.c. This function only needs to be called at the
////             * rate requested by the host.
////             */
////            read_from_mpl();
////        }
////    }
//}

